#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
#include "ros/ros.h"
#include <string>
#include "std_msgs/String.h"
#define SERVER_PORT 3000
#define SERVER_IP "192.168.224.101"
/*send NMEA message*/
int sock;
struct sockaddr_in server;

unsigned char check_data(unsigned char *data, int length);

void printf_array(unsigned char *buffer, int length)
{
	printf("*************printf array****************\r\n");
	for (int i = 0; i < length; i++)
	{
		printf("%02x ", buffer[i]);
	}
	printf("\r\n");
}

void sendGPS(int sock, struct sockaddr_in server, std::string s)
{
	ssize_t readlength = 0;
	char gpsbuf[1024];
	memset(gpsbuf, 0, sizeof(gpsbuf));
	memcpy(gpsbuf, s.data(), s.size());
    unsigned char buf[1024];  
	buf[0] = 0xAA;
	buf[1] = 0x75;
	buf[2] = 0x01;
	buf[3] = 0x00;
	buf[4] = strlen(gpsbuf);
	memcpy(buf + 5, gpsbuf, strlen(gpsbuf));
	buf[5 + strlen(gpsbuf)] = check_data(buf, 5 + strlen(gpsbuf));

	printf("now send gps data# %s \r\n", gpsbuf);
	sendto(sock, buf, 6 + strlen(gpsbuf), 0, (struct sockaddr *)&server, sizeof(server));
	readlength = recvfrom(sock, buf, sizeof(buf), 0, NULL, NULL);
	printf("server ack# \r\n");
	printf_array(buf, readlength);
}

unsigned char check_data(unsigned char *data, int length)
{
	unsigned char result = 0;
	result = data[0];

	for (int i = 1; i < length; i++)
	{
		result ^= data[i];
	}

	return result;
}

void callback(const std_msgs::String& msg){
	ROS_INFO("I heard: [%s]", msg.data.data());
 	sendGPS(sock,server,msg.data);
	
}
int main(int argc, char **argv)
{
	ros::init(argc, argv ,"udp_send");
	ROS_INFO("INIT");
	ros::NodeHandle n;	
	ros::Subscriber sub = n.subscribe("gps", 1, callback);
	sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (sock < 0)
	{
		perror("socket");
	}
	server.sin_family = AF_INET;
	server.sin_port = htons(SERVER_PORT);
	server.sin_addr.s_addr = inet_addr(SERVER_IP); //INADDR_ANY;
	ros::spin();
	close(sock);
	return 0;
}
